from math import floor,pow,sqrt
from constants import *

class Node():
    def __init__(self, x, y, parent_index, g, h, f):
        self.x = x;
        self.y = y;
        self.parent_index = parent_index;
        self.g = g;
        self.h = h;
        self.f = f;




def a_star(start_cell, destination_cell, board, forbiden_position):
    #Create start and destination as true nodes
    start = Node(start_cell.i, start_cell.j, -1, -1, -1, -1)
    destination = Node(destination_cell.i, destination_cell.j, -1, -1, -1, -1)
    #destination = Node(20, 14, -1, -1, -1, -1)

    openlist = [] #List of open nodes (nodes to be inspected)
    closedlist = [] #List of closed nodes (nodes we've already inspected)

    #Push the start node onto the list of open nodes
    openlist.append(start)

    #Keep going while there's nodes in our open list
    while openlist.__len__() > 0:
        #Find the best open node (lowest f value)

        #Alternately, you could simply keep the open list sorted by f value lowest to highest,
        #in which case you always use the first node
        best_cost = openlist[0].f
        best_node = openlist[0]
        for node in openlist:
            if (node.f < best_cost):
                best_cost = node.f
                best_node = node

        #Set it as our current node
        current_node = best_node
        #print current_node.parent_index

        #Check if we've reached our destination
        if (current_node.x == destination.x and current_node.y == destination.y):
            path = [destination]; # Initialize the path with the destination node
            # print "destino encontrado!"

            # Go up the chain to recreate the path 
            while (current_node.parent_index != -1):
                #print current_node.parent_index
                current_node = closedlist[current_node.parent_index]
                path.insert(0, current_node)

            return path;

        # Remove the current node from our open list
        openlist.remove(current_node)
        # Push it onto the closed list
        closedlist.append(current_node);

        adjacentlist=[(current_node.x-1, current_node.y),(current_node.x+1, current_node.y),(current_node.x, current_node.y+1),(current_node.x, current_node.y-1)]
        #print adjacentlist
        # Expand our current node (look in all 8 directions)
        for neighbor in adjacentlist:
                if forbiden_position: 
                    if neighbor[0] == forbiden_position[0] and neighbor[1] == forbiden_position[1]:
                        continue 
                
                # If the new node is open
                if board[neighbor[0]][neighbor[1]] == 0 or destination.x == neighbor[0] and destination.y == neighbor[1]: #or the new node is our destination
                    # See if the node is already in our closed list. If so, skip it.
                    found_in_closed = False
                    for node in closedlist:
                        if node.x == neighbor[0] and node.y == neighbor[1]:
                            found_in_closed = True
                            break

                    if found_in_closed:
                        continue

                    # See if the node is in our open list. If not, use it.
                    found_in_open = False
                    for node in openlist:
                        if node.x == neighbor[0] and node.y == neighbor[1]:
                            found_in_open = True
                            break

                    if not found_in_open:
                        new_node = Node(neighbor[0], neighbor[1], closedlist.__len__()-1, -1, -1, -1);

                        new_node.g = current_node.g + floor(sqrt(pow(new_node.x-current_node.x, 2)+pow(new_node.y-current_node.y, 2)))
                        new_node.h = heuristic(new_node, destination)
                        new_node.f = new_node.g+new_node.h
                        
                        openlist.append(new_node)

    # print "destino NO encontrado!"           
    return []
    




#An A* heurisitic must be admissible, meaning it must never overestimate the distance to the goal.
#In other words, it must either underestimate or return exactly the distance to the goal.
def heuristic(current_node, destination):
    #Find the straight-line distance between the current node and the destination. (Thanks to id for the improvement)
    #return Math.floor(Math.sqrt(Math.pow(current_node.x-destination.x, 2)+Math.pow(current_node.y-destination.y, 2)));
    x = current_node.x-destination.x;
    y = current_node.y-destination.y;
    return x*x+y*y;

def tiledmap2board(tiledmap, start_point, end_point, prop):
    board = []
    for i in range(end_point[0]-start_point[0]):
        board.insert(i, []) 
        for j in range(end_point[1]-start_point[1]):
            c = tiledmap.get_cell(start_point[1]+i, start_point[0]+j)
            if c.get(prop):
                value=1
            else:
                value=0
            board[i].insert(j, value)
    return board

def validateDestinationOutside(cell_pos, tile_map):
    
    cell_pos = tile_map.get_at_pixel(cell_pos[0], cell_pos[1])
    #celda = tile_map.get_cell(cell_pos.i, cell_pos.j)
    return_cell_pos = [cell_pos.i, cell_pos.j]
#     if celda.get('valid'):
#         print 'es valido'
#         return return_cell_pos
    if cell_pos.i < limitInfX:
        return_cell_pos[0] = limitInfX
    if cell_pos.i > limitSupY:
        return_cell_pos[0] = limitSupX
    if cell_pos.j < limitInfY:
        return_cell_pos[1] = limitInfY
    if cell_pos.j > limitSupY:
        return_cell_pos[1] = limitSupY
    return return_cell_pos
        
def validateDestinationInside(cell_pos, tile_map):
    offset = (0, 0)
    cell_pos = tile_map.get_cell(cell_pos[0], cell_pos[1])
    if cell_pos.get('wall'):
        offset = getWallDepth(cell_pos, tile_map)
    return (cell_pos.i + offset[0], cell_pos.j + offset[1])

def getWallDepth(cell_pos, tile_map):
    adjacentsOffset = [(-1, 0), (1, 0), (0, 1), (0, -1)]
    wallDepth = [0, 0, 0, 0]
    offsetIndex = -1
    minDepth = maxWallDepth
    
    #adjacents = [(cell.i-1, cell.j), (cell.i+1, cell.j), (cell.i, cell.j+1), (cell.i, cell.j-1)]
    for offset in adjacentsOffset:
        offsetIndex += 1
        if ( (cell_pos.i + offset[0] < limitInfX) or (cell_pos.i + offset[0] > limitSupX) ):
            wallDepth[offsetIndex] = maxWallDepth
            continue
        if ( (cell_pos.j + offset[1] < limitInfY) or (cell_pos.j + offset[1] > limitSupY) ):
            wallDepth[offsetIndex] = maxWallDepth
            continue
        
        idxi = cell_pos.i + offset[0] 
        idxj = cell_pos.j + offset[1]
        while tile_map.get_cell(idxi, idxj).get('wall'):
            wallDepth[offsetIndex] += 1
            idxi += offset[0]
            idxj += offset[1]

    
    for wi in range( wallDepth.__len__() ):
        if wallDepth[wi] < minDepth:
            minDepth = wallDepth[wi]
            minIndex = wi
            
    a = (minDepth*adjacentsOffset[minIndex][0], minDepth*adjacentsOffset[minIndex][1])
    return a